using dtPolyRef = System.UInt32;
using dtStatus = System.UInt32;
using System.Runtime.CompilerServices;
using System;

namespace Recast
{
	public unsafe partial class RecastGlobal
	{
		public unsafe partial class dtNavMeshQuery
		{
			public dtNavMesh _nav;
			public dtNodePool _tinyNodePool;
			public dtNodePool _nodePool;
			public dtNodeQueue _openList;
			public dtFindNearestPolyQuery _query;
			public dtCollectPolysQuery _collector;

			public dtNavMesh* m_nav
			{
				get => _nav == null ? null : (dtNavMesh*)Unsafe.AsPointer(ref _nav);
				set => _nav = value != null ? *value : null;
			}
			public dtQueryData m_query;
			public dtNodePool* m_tinyNodePool
			{
				get => _tinyNodePool == null ? null : (dtNodePool*)Unsafe.AsPointer(ref _tinyNodePool);
				set => _tinyNodePool = value != null ? *value : null;
			}
			public dtNodePool* m_nodePool
			{
				get => _nodePool == null ? null : (dtNodePool*)Unsafe.AsPointer(ref _nodePool);
				set => _nodePool = value != null ? *value : null;
			}
			public dtNodeQueue* m_openList
			{
				get => _openList == null ? null : (dtNodeQueue*)Unsafe.AsPointer(ref _openList);
				set => _openList = value != null ? *value : null;
			}
			public dtFindNearestPolyQuery* query
			{
				get => _query == null ? null : (dtFindNearestPolyQuery*)Unsafe.AsPointer(ref _query);
				set => _query = value != null ? *value : null;
			}
			public dtCollectPolysQuery* collector
			{
				get => _collector == null ? null : (dtCollectPolysQuery*)Unsafe.AsPointer(ref _collector);
				set => _collector = value != null ? *value : null;
			}

			public dtNavMeshQuery()
			{
				memset(Unsafe.AsPointer(ref m_query), 0, sizeof(dtQueryData));
				_query = new dtFindNearestPolyQuery(this, null);
				_collector = new dtCollectPolysQuery(null, 0);
			}
			~dtNavMeshQuery()
			{
				rcDelete(m_tinyNodePool);
				rcDelete(m_nodePool);
				rcDelete(m_openList);
			}
			public dtStatus init(dtNavMesh* nav, int maxNodes)
			{
				if (maxNodes > DT_NULL_IDX || maxNodes > (1 << DT_NODE_PARENT_BITS) - 1)
					return DT_FAILURE | DT_INVALID_PARAM;
				//m_nav = nav;
				_nav = nav != null ? *nav : null;
				if (null == m_nodePool || m_nodePool->getMaxNodes() < maxNodes)
				{
					if (null != m_nodePool)
					{
						//m_nodePool->Dispose();
						dtFree(m_nodePool);
						m_nodePool = null;
					}
					//m_nodePool = (dtNodePool*)dtAlloc(sizeof(dtNodePool), dtAllocHint.DT_ALLOC_PERM);
					_nodePool = new dtNodePool(maxNodes, (int)dtNextPow2((uint)(maxNodes / 4)));
					//AddObjHook(_nodePool);
					if (null == m_nodePool)
						return DT_FAILURE | DT_OUT_OF_MEMORY;
				}
				else
				{
					m_nodePool->clear();
				}
				if (null == m_tinyNodePool)
				{
					//m_tinyNodePool = (dtNodePool*)dtAlloc(sizeof(dtNodePool), dtAllocHint.DT_ALLOC_PERM);
					_tinyNodePool = new dtNodePool(64, 32);
					//AddObjHook(_tinyNodePool);
					if (null == m_tinyNodePool)
						return DT_FAILURE | DT_OUT_OF_MEMORY;
				}
				else
				{
					m_tinyNodePool->clear();
				}
				if (null == m_openList || m_openList->getCapacity() < maxNodes)
				{
					if (null != m_openList)
					{
						//m_openList->Dispose();
						dtFree(m_openList);
						m_openList = null;
					}
					//m_openList = (dtNodeQueue*)dtAlloc(sizeof(dtNodeQueue), dtAllocHint.DT_ALLOC_PERM);//new(dtAlloc(sizeof(dtNodeQueue), dtAllocHint.DT_ALLOC_PERM))dtNodeQueue(maxNodes);
					_openList = new dtNodeQueue(maxNodes);
					//AddObjHook(_openList);
					if (null == m_openList)
						return DT_FAILURE | DT_OUT_OF_MEMORY;
				}
				else
				{
					m_openList->clear();
				}
				return DT_SUCCESS;
			}
			public dtStatus findPath(dtPolyRef startRef, dtPolyRef endRef, float* startPos, float* endPos, dtQueryFilter* filter, dtPolyRef* path, int* pathCount, int maxPath)
			{
				dtAssert(m_nav);
				dtAssert(m_nodePool);
				dtAssert(m_openList);
				if (null == pathCount)
					return DT_FAILURE | DT_INVALID_PARAM;
				*pathCount = 0;
				if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) ||
				null == startPos || !dtVisfinite(startPos) ||
				null == endPos || !dtVisfinite(endPos) ||
				null == filter || null == path || maxPath <= 0)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				if (startRef == endRef)
				{
					path[0] = startRef;
					*pathCount = 1;
					return DT_SUCCESS;
				}
				m_nodePool->clear();
				m_openList->clear();
				dtNode* startNode = m_nodePool->getNode(startRef);
				dtVcopy(startNode->pos, startPos);
				startNode->pidx = 0;
				startNode->cost = 0;
				startNode->total = dtVdist(startPos, endPos) * H_SCALE;
				startNode->id = startRef;
				startNode->flags = (uint)dtNodeFlags.DT_NODE_OPEN;
				m_openList->push(startNode);
				dtNode* lastBestNode = startNode;
				float lastBestNodeCost = startNode->total;
				bool OutOfNodes = false;
				while (!m_openList->empty())
				{
					dtNode* bestNode = m_openList->pop();
					bestNode->flags &= ~(uint)dtNodeFlags.DT_NODE_OPEN;
					bestNode->flags |= (uint)dtNodeFlags.DT_NODE_CLOSED;
					if (bestNode->id == endRef)
					{
						lastBestNode = bestNode;
						break;
					}
					dtPolyRef bestRef = bestNode->id;
					dtMeshTile* bestTile = (dtMeshTile*)0;
					dtPoly* bestPoly = (dtPoly*)0;
					m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
					dtPolyRef parentRef = 0;
					dtMeshTile* parentTile = (dtMeshTile*)0;
					dtPoly* parentPoly = (dtPoly*)0;
					if (0 != bestNode->pidx)
						parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
					if (0 != parentRef)
						m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
					for (uint i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
					{
						dtPolyRef neighbourRef = bestTile->links[i].Ref;
						if (0 == neighbourRef || neighbourRef == parentRef)
							continue;
						dtMeshTile* neighbourTile = (dtMeshTile*)0;
						dtPoly* neighbourPoly = (dtPoly*)0;
						m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
						if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
							continue;
						byte crossSide = 0;
						if (bestTile->links[i].side != 0xff)
							crossSide = (byte)(bestTile->links[i].side >> 1);
						dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide);
						if (null == neighbourNode)
						{
							OutOfNodes = true;
							continue;
						}
						if (neighbourNode->flags == 0)
						{
							getEdgeMidPoint(bestRef, bestPoly, bestTile,
							neighbourRef, neighbourPoly, neighbourTile,
							neighbourNode->pos);
						}
						float cost = 0;
						float heuristic = 0;
						if (neighbourRef == endRef)
						{
							float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
							parentRef, parentTile, parentPoly,
							bestRef, bestTile, bestPoly,
							neighbourRef, neighbourTile, neighbourPoly);
							float endCost = filter->getCost(neighbourNode->pos, endPos,
							bestRef, bestTile, bestPoly,
							neighbourRef, neighbourTile, neighbourPoly,
							0, (dtMeshTile*)0, (dtPoly*)0);
							cost = bestNode->cost + curCost + endCost;
							heuristic = 0;
						}
						else
						{
							float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
							parentRef, parentTile, parentPoly,
							bestRef, bestTile, bestPoly,
							neighbourRef, neighbourTile, neighbourPoly);
							cost = bestNode->cost + curCost;
							heuristic = dtVdist(neighbourNode->pos, endPos) * H_SCALE;
						}
						float total = cost + heuristic;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN) && total >= neighbourNode->total)
							continue;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_CLOSED) && total >= neighbourNode->total)
							continue;
						neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
						neighbourNode->id = neighbourRef;
						neighbourNode->flags = neighbourNode->flags & ~(uint)dtNodeFlags.DT_NODE_CLOSED;
						neighbourNode->cost = cost;
						neighbourNode->total = total;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN))
						{
							m_openList->modify(neighbourNode);
						}
						else
						{
							neighbourNode->flags |= (uint)dtNodeFlags.DT_NODE_OPEN;
							m_openList->push(neighbourNode);
						}
						if (heuristic < lastBestNodeCost)
						{
							lastBestNodeCost = heuristic;
							lastBestNode = neighbourNode;
						}
					}
				}
				dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath);
				if (lastBestNode->id != endRef)
					status |= DT_PARTIAL_RESULT;
				if (OutOfNodes)
					status |= DT_OUT_OF_NODES;
				return status;
			}
			public dtStatus updateSlicedFindPath(int maxIter, int* doneIters)
			{
				if (!dtStatusInProgress(m_query.status))
					return m_query.status;
				if (!m_nav->isValidPolyRef(m_query.startRef) || !m_nav->isValidPolyRef(m_query.endRef))
				{
					m_query.status = DT_FAILURE;
					return DT_FAILURE;
				}
				dtRaycastHit rayHit;
				rayHit.maxPath = 0;
				int iter = 0;
				while (iter < maxIter && !m_openList->empty())
				{
					iter++;
					dtNode* bestNode = m_openList->pop();
					bestNode->flags &= ~(uint)dtNodeFlags.DT_NODE_OPEN;
					bestNode->flags |= (uint)dtNodeFlags.DT_NODE_CLOSED;
					if (bestNode->id == m_query.endRef)
					{
						m_query.lastBestNode = bestNode;
						dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
						m_query.status = DT_SUCCESS | details;
						if (null != doneIters)
							*doneIters = iter;
						return m_query.status;
					}
					dtPolyRef bestRef = bestNode->id;
					dtMeshTile* bestTile = (dtMeshTile*)0;
					dtPoly* bestPoly = (dtPoly*)0;
					if (dtStatusFailed(m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly)))
					{
						m_query.status = DT_FAILURE;
						if (null != doneIters)
							*doneIters = iter;
						return m_query.status;
					}
					dtPolyRef parentRef = 0, grandpaRef = 0;
					dtMeshTile* parentTile = (dtMeshTile*)0;
					dtPoly* parentPoly = (dtPoly*)0;
					dtNode* parentNode = (dtNode*)0;
					if (0 != bestNode->pidx)
					{
						parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx);
						parentRef = parentNode->id;
						if (0 != parentNode->pidx)
							grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id;
					}
					if (0 != parentRef)
					{
						bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly));
						if (invalidParent || (0 != grandpaRef && !m_nav->isValidPolyRef(grandpaRef)))
						{
							m_query.status = DT_FAILURE;
							if (null != doneIters)
								*doneIters = iter;
							return m_query.status;
						}
					}
					bool tryLOS = false;
					if (0 != (m_query.options & (uint)dtFindPathOptions.DT_FINDPATH_ANY_ANGLE))
					{
						if ((parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < m_query.raycastLimitSqr))
							tryLOS = true;
					}
					for (uint i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
					{
						dtPolyRef neighbourRef = bestTile->links[i].Ref;
						if (0 == neighbourRef || neighbourRef == parentRef)
							continue;
						dtMeshTile* neighbourTile = (dtMeshTile*)0;
						dtPoly* neighbourPoly = (dtPoly*)0;
						m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
						if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
							continue;
						dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, 0);
						if (null == neighbourNode)
						{
							m_query.status |= DT_OUT_OF_NODES;
							continue;
						}
						if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx)
							continue;
						if (neighbourNode->flags == 0)
						{
							getEdgeMidPoint(bestRef, bestPoly, bestTile,
							neighbourRef, neighbourPoly, neighbourTile,
							neighbourNode->pos);
						}
						float cost = 0;
						float heuristic = 0;
						bool foundShortCut = false;
						rayHit.pathCost = rayHit.t = 0;
						if (tryLOS)
						{
							raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, (uint)dtRaycastOptions.DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef);
							foundShortCut = rayHit.t >= 1.0f;
						}
						if (foundShortCut)
						{
							cost = parentNode->cost + rayHit.pathCost;
						}
						else
						{
							float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
							parentRef, parentTile, parentPoly,
						   bestRef, bestTile, bestPoly,
						   neighbourRef, neighbourTile, neighbourPoly);
							cost = bestNode->cost + curCost;
						}
						fixed (float* endPos = m_query.endPos)
						{
							if (neighbourRef == m_query.endRef)
							{
								float endCost = m_query.filter->getCost(neighbourNode->pos, endPos,
								bestRef, bestTile, bestPoly,
								neighbourRef, neighbourTile, neighbourPoly,
								0, (dtMeshTile*)0, (dtPoly*)0);
								cost += endCost;
								heuristic = 0;
							}
							else
							{
								heuristic = dtVdist(neighbourNode->pos, endPos) * H_SCALE;
							}
						}
						float total = cost + heuristic;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN) && total >= neighbourNode->total)
							continue;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_CLOSED) && total >= neighbourNode->total)
							continue;
						neighbourNode->pidx = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode);
						neighbourNode->id = neighbourRef;
						neighbourNode->flags = (neighbourNode->flags & ~((uint)dtNodeFlags.DT_NODE_CLOSED | (uint)dtNodeFlags.DT_NODE_PARENT_DETACHED));
						neighbourNode->cost = cost;
						neighbourNode->total = total;
						if (foundShortCut)
							neighbourNode->flags = (neighbourNode->flags | (uint)dtNodeFlags.DT_NODE_PARENT_DETACHED);
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN))
						{
							m_openList->modify(neighbourNode);
						}
						else
						{
							neighbourNode->flags |= (uint)dtNodeFlags.DT_NODE_OPEN;
							m_openList->push(neighbourNode);
						}
						if (heuristic < m_query.lastBestNodeCost)
						{
							m_query.lastBestNodeCost = heuristic;
							m_query.lastBestNode = neighbourNode;
						}
					}
				}
				if (m_openList->empty())
				{
					dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
					m_query.status = DT_SUCCESS | details;
				}
				if (null != doneIters)
					*doneIters = iter;
				return m_query.status;
			}
			public dtStatus finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, int maxPath)
			{
				if (null == pathCount)
					return DT_FAILURE | DT_INVALID_PARAM;
				*pathCount = 0;
				if (null == path || maxPath <= 0)
					return DT_FAILURE | DT_INVALID_PARAM;
				if (dtStatusFailed(m_query.status))
				{
					memset(Unsafe.AsPointer(ref m_query), 0, sizeof(dtQueryData));
					return DT_FAILURE;
				}
				int n = 0;
				if (m_query.startRef == m_query.endRef)
				{
					path[n++] = m_query.startRef;
				}
				else
				{
					dtAssert(m_query.lastBestNode);
					if (m_query.lastBestNode->id != m_query.endRef)
						m_query.status |= DT_PARTIAL_RESULT;
					dtNode* prev = (dtNode*)0;
					dtNode* node = m_query.lastBestNode;
					int prevRay = 0;
					do
					{
						dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
						node->pidx = m_nodePool->getNodeIdx(prev);
						prev = node;
						int nextRay = (int)(node->flags & (uint)dtNodeFlags.DT_NODE_PARENT_DETACHED);
						node->flags = (node->flags & ~(uint)dtNodeFlags.DT_NODE_PARENT_DETACHED) | (uint)prevRay;
						prevRay = nextRay;
						node = next;
					}
					while (null != node);
					node = prev;
					float* normal = stackalloc float[3];
					do
					{
						dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
						dtStatus status = 0;
						if (0 != (node->flags & (uint)dtNodeFlags.DT_NODE_PARENT_DETACHED))
						{
							float* t = (float*)0;
							//float* normal = stackalloc float[3];
							memset(normal, 0, sizeof(float) * 3);
							int m;
							status = raycast(node->id, node->pos, next->pos, m_query.filter, t, normal, path + n, &m, maxPath - n);
							n += m;
							if (path[n - 1] == next->id)
								n--;
						}
						else
						{
							path[n++] = node->id;
							if (n >= maxPath)
								status = DT_BUFFER_TOO_SMALL;
						}
						if (0 != (status & DT_STATUS_DETAIL_MASK))
						{
							m_query.status |= status & DT_STATUS_DETAIL_MASK;
							break;
						}
						node = next;
					}
					while (null != node);
				}
				dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
				memset(Unsafe.AsPointer(ref m_query), 0, sizeof(dtQueryData));
				*pathCount = n;
				return DT_SUCCESS | details;
			}
			public dtStatus finalizeSlicedFindPathPartial(dtPolyRef* existing, int existingSize, dtPolyRef* path, int* pathCount, int maxPath)
			{
				if (null == pathCount)
					return DT_FAILURE | DT_INVALID_PARAM;
				*pathCount = 0;
				if (null == existing || existingSize <= 0 || null == path || null == pathCount || maxPath <= 0)
					return DT_FAILURE | DT_INVALID_PARAM;
				if (dtStatusFailed(m_query.status))
				{
					memset(Unsafe.AsPointer(ref m_query), 0, sizeof(dtQueryData));
					return DT_FAILURE;
				}
				int n = 0;
				if (m_query.startRef == m_query.endRef)
				{
					path[n++] = m_query.startRef;
				}
				else
				{
					dtNode* prev = (dtNode*)0;
					dtNode* node = (dtNode*)0;
					for (int i = existingSize - 1; i >= 0; --i)
					{
						m_nodePool->findNodes(existing[i], node, 1);
						if (null != node)
							break;
					}
					if (null == node)
					{
						m_query.status |= DT_PARTIAL_RESULT;
						dtAssert(m_query.lastBestNode);
						node = m_query.lastBestNode;
					}
					int prevRay = 0;
					do
					{
						dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
						node->pidx = m_nodePool->getNodeIdx(prev);
						prev = node;
						int nextRay = (int)(node->flags & (uint)dtNodeFlags.DT_NODE_PARENT_DETACHED);
						node->flags = (uint)((node->flags & ~(uint)dtNodeFlags.DT_NODE_PARENT_DETACHED) | (uint)prevRay);
						prevRay = nextRay;
						node = next;
					}
					while (null != node);
					node = prev;
					float* normal = stackalloc float[3];
					do
					{
						dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
						dtStatus status = 0;
						if ((node->flags & (uint)dtNodeFlags.DT_NODE_PARENT_DETACHED) != 0)
						{
							float* t = (float*)0;
							//float* normal = stackalloc float[3];
							memset(normal, 0, sizeof(float) * 3);
							int m;
							status = raycast(node->id, node->pos, next->pos, m_query.filter, t, normal, path + n, &m, maxPath - n);
							n += m;
							if (path[n - 1] == next->id)
								n--;
						}
						else
						{
							path[n++] = node->id;
							if (n >= maxPath)
								status = DT_BUFFER_TOO_SMALL;
						}
						if (0 != (status & DT_STATUS_DETAIL_MASK))
						{
							m_query.status |= status & DT_STATUS_DETAIL_MASK;
							break;
						}
						node = next;
					}
					while (null != node);
				}
				dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
				memset(Unsafe.AsPointer(ref m_query), 0, sizeof(dtQueryData));
				*pathCount = n;
				return DT_SUCCESS | details;
			}
			public dtStatus findPolysAroundCircle(dtPolyRef startRef, float* centerPos, float radius, dtQueryFilter* filter, dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, int* resultCount, int maxResult)
			{
				dtAssert(m_nav);
				dtAssert(m_nodePool);
				dtAssert(m_openList);
				if (null == resultCount)
					return DT_FAILURE | DT_INVALID_PARAM;
				*resultCount = 0;
				if (!m_nav->isValidPolyRef(startRef) ||
				null == centerPos || !dtVisfinite(centerPos) ||
				radius < 0 || !dtMathIsfinite(radius) ||
				null == filter || maxResult < 0)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				m_nodePool->clear();
				m_openList->clear();
				dtNode* startNode = m_nodePool->getNode(startRef);
				dtVcopy(startNode->pos, centerPos);
				startNode->pidx = 0;
				startNode->cost = 0;
				startNode->total = 0;
				startNode->id = startRef;
				startNode->flags = (uint)dtNodeFlags.DT_NODE_OPEN;
				m_openList->push(startNode);
				dtStatus status = DT_SUCCESS;
				int n = 0;
				float radiusSqr = dtSqr(radius);
				float* va = stackalloc float[3], vb = stackalloc float[3];
				while (!m_openList->empty())
				{
					dtNode* bestNode = m_openList->pop();
					bestNode->flags &= ~(uint)dtNodeFlags.DT_NODE_OPEN;
					bestNode->flags |= (uint)dtNodeFlags.DT_NODE_CLOSED;
					dtPolyRef bestRef = bestNode->id;
					dtMeshTile* bestTile = (dtMeshTile*)0;
					dtPoly* bestPoly = (dtPoly*)0;
					m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
					dtPolyRef parentRef = 0;
					dtMeshTile* parentTile = (dtMeshTile*)0;
					dtPoly* parentPoly = (dtPoly*)0;
					if (0 != bestNode->pidx)
						parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
					if (0 != parentRef)
						m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
					if (n < maxResult)
					{
						if (null != resultRef)
							resultRef[n] = bestRef;
						if (null != resultParent)
							resultParent[n] = parentRef;
						if (null != resultCost)
							resultCost[n] = bestNode->total;
						++n;
					}
					else
					{
						status |= DT_BUFFER_TOO_SMALL;
					}
					for (uint i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
					{
						dtLink* link = &bestTile->links[i];
						dtPolyRef neighbourRef = link->Ref;
						if (0 == neighbourRef || neighbourRef == parentRef)
							continue;
						dtMeshTile* neighbourTile = (dtMeshTile*)0;
						dtPoly* neighbourPoly = (dtPoly*)0;
						m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
						if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
							continue;
						//float* va = stackalloc float[3], vb = stackalloc float[3];
						memset(va, 0, sizeof(float) * 3);
						memset(vb, 0, sizeof(float) * 3);
						if (0 == getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
							continue;
						float tseg = 0f;
						float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, ref tseg);
						if (distSqr > radiusSqr)
							continue;
						dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
						if (null == neighbourNode)
						{
							status |= DT_OUT_OF_NODES;
							continue;
						}
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_CLOSED))
							continue;
						if (neighbourNode->flags == 0)
							dtVlerp(neighbourNode->pos, va, vb, 0.5f);
						float cost = filter->getCost(
						bestNode->pos, neighbourNode->pos,
						parentRef, parentTile, parentPoly,
						bestRef, bestTile, bestPoly,
						neighbourRef, neighbourTile, neighbourPoly);
						float total = bestNode->total + cost;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN) && total >= neighbourNode->total)
							continue;
						neighbourNode->id = neighbourRef;
						neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
						neighbourNode->total = total;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN))
						{
							m_openList->modify(neighbourNode);
						}
						else
						{
							neighbourNode->flags = (uint)dtNodeFlags.DT_NODE_OPEN;
							m_openList->push(neighbourNode);
						}
					}
				}
				*resultCount = n;
				return status;
			}
			public dtStatus findPolysAroundShape(dtPolyRef startRef, float* verts, int nverts, dtQueryFilter* filter, dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, int* resultCount, int maxResult)
			{
				dtAssert(m_nav);
				dtAssert(m_nodePool);
				dtAssert(m_openList);
				if (null == resultCount)
					return DT_FAILURE | DT_INVALID_PARAM;
				*resultCount = 0;
				if (!m_nav->isValidPolyRef(startRef) ||
				null == verts || nverts < 3 ||
				null == filter || maxResult < 0)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				if (0 == startRef || !m_nav->isValidPolyRef(startRef))
					return DT_FAILURE | DT_INVALID_PARAM;
				m_nodePool->clear();
				m_openList->clear();
				float* centerPos = stackalloc float[3] { 0, 0, 0 };
				for (int i = 0; i < nverts; ++i)
					dtVadd(centerPos, centerPos, &verts[i * 3]);
				dtVscale(centerPos, centerPos, 1.0f / nverts);
				dtNode* startNode = m_nodePool->getNode(startRef);
				dtVcopy(startNode->pos, centerPos);
				startNode->pidx = 0;
				startNode->cost = 0;
				startNode->total = 0;
				startNode->id = startRef;
				startNode->flags = (uint)dtNodeFlags.DT_NODE_OPEN;
				m_openList->push(startNode);
				dtStatus status = DT_SUCCESS;
				int n = 0;
				float* va = stackalloc float[3], vb = stackalloc float[3];
				while (!m_openList->empty())
				{
					dtNode* bestNode = m_openList->pop();
					bestNode->flags &= ~(uint)dtNodeFlags.DT_NODE_OPEN;
					bestNode->flags |= (uint)dtNodeFlags.DT_NODE_CLOSED;
					dtPolyRef bestRef = bestNode->id;
					dtMeshTile* bestTile = (dtMeshTile*)0;
					dtPoly* bestPoly = (dtPoly*)0;
					m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
					dtPolyRef parentRef = 0;
					dtMeshTile* parentTile = null;
					dtPoly* parentPoly = null;
					if (0 != bestNode->pidx)
						parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
					if (0 != parentRef)
						m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
					if (n < maxResult)
					{
						if (null != resultRef)
							resultRef[n] = bestRef;
						if (null != resultParent)
							resultParent[n] = parentRef;
						if (null != resultCost)
							resultCost[n] = bestNode->total;
						++n;
					}
					else
					{
						status |= DT_BUFFER_TOO_SMALL;
					}
					for (uint i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
					{
						dtLink* link = &bestTile->links[i];
						dtPolyRef neighbourRef = link->Ref;
						if (0 == neighbourRef || neighbourRef == parentRef)
							continue;
						dtMeshTile* neighbourTile = (dtMeshTile*)0;
						dtPoly* neighbourPoly = (dtPoly*)0;
						m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
						if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
							continue;
						//float* va = stackalloc float[3], vb = stackalloc float[3];
						memset(va, 0, sizeof(float) * 3);
						memset(vb, 0, sizeof(float) * 3);
						if (0 == getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
							continue;
						float tmin = 0f, tmax = 0f;
						int segMin = 0, segMax = 0;
						if (!dtIntersectSegmentPoly2D(va, vb, verts, nverts, ref tmin, ref tmax, ref segMin, ref segMax))
							continue;
						if (tmin > 1.0f || tmax < 0.0f)
							continue;
						dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
						if (null == neighbourNode)
						{
							status |= DT_OUT_OF_NODES;
							continue;
						}
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_CLOSED))
							continue;
						if (neighbourNode->flags == 0)
							dtVlerp(neighbourNode->pos, va, vb, 0.5f);
						float cost = filter->getCost(
						bestNode->pos, neighbourNode->pos,
						parentRef, parentTile, parentPoly,
						bestRef, bestTile, bestPoly,
						neighbourRef, neighbourTile, neighbourPoly);
						float total = bestNode->total + cost;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN) && total >= neighbourNode->total)
							continue;
						neighbourNode->id = neighbourRef;
						neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
						neighbourNode->total = total;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN))
						{
							m_openList->modify(neighbourNode);
						}
						else
						{
							neighbourNode->flags = (uint)dtNodeFlags.DT_NODE_OPEN;
							m_openList->push(neighbourNode);
						}
					}
				}
				*resultCount = n;
				return status;
			}
			public dtStatus getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath)
			{
				if (!m_nav->isValidPolyRef(endRef) || null == path || null == pathCount || maxPath < 0)
					return DT_FAILURE | DT_INVALID_PARAM;
				*pathCount = 0;
				dtNode* endNode = (dtNode*)0;
				if (m_nodePool->findNodes(endRef, endNode, 1) != 1 ||
				(endNode->flags & (uint)dtNodeFlags.DT_NODE_CLOSED) == 0)
					return DT_FAILURE | DT_INVALID_PARAM;
				return getPathToNode(endNode, path, pathCount, maxPath);
			}
			public dtStatus findNearestPoly(float* center, float* halfExtents, dtQueryFilter* filter, ref dtPolyRef nearestRef, float* nearestPt)
			{
				return findNearestPoly(center, halfExtents, filter, ref nearestRef, nearestPt, null);
			}

			public dtStatus findNearestPoly(float* center, float* halfExtents, dtQueryFilter* filter, ref dtPolyRef nearestRef, float* nearestPt, bool* isOverPoly)
			{
				dtAssert(m_nav);
				//dtFindNearestPolyQuery* query = (dtFindNearestPolyQuery*)rcAlloc(sizeof(dtFindNearestPolyQuery), rcAllocHint.RC_ALLOC_TEMP);
				//*query = new dtFindNearestPolyQuery(this, center);
				query->m_query = this;
				query->m_center = center;
				query->m_nearestDistanceSqr = FLT_MAX;
				query->m_nearestRef = 0;
				query->m_overPoly = false;
				query->m_nearestPoint[0] = 0;
				query->m_nearestPoint[1] = 0;
				query->m_nearestPoint[2] = 0;
				dtStatus status = queryPolygons(center, halfExtents, filter, (dtPolyQuery*)query);
				if (dtStatusFailed(status))
					return status;
				nearestRef = query->nearestRef();
				if (null != nearestPt && 0 != nearestRef)
				{
					dtVcopy(nearestPt, query->nearestPoint());
					if (null != isOverPoly)
						*isOverPoly = query->isOverPoly();
				}
				//query->Dispose();
				//rcFree(query);
				return DT_SUCCESS;
			}
			public dtStatus queryPolygons(float* center, float* halfExtents, dtQueryFilter* filter, dtPolyRef* polys, int* polyCount, int maxPolys)
			{
				if (null == polys || null == polyCount || maxPolys < 0)
					return DT_FAILURE | DT_INVALID_PARAM;
				//dtCollectPolysQuery* collector = (dtCollectPolysQuery*)dtAlloc(sizeof(dtCollectPolysQuery), dtAllocHint.DT_ALLOC_PERM);
				//*collector = new dtCollectPolysQuery(polys, maxPolys);
				collector->m_polys = polys;
				collector->m_maxPolys = maxPolys;
				collector->m_numCollected = 0;
				collector->m_overflow = false;
				dtStatus status = queryPolygons(center, halfExtents, filter, (dtPolyQuery*)collector);
				if (dtStatusFailed(status))
					return status;
				*polyCount = collector->numCollected();
				return collector->overflowed() ? DT_SUCCESS | DT_BUFFER_TOO_SMALL : DT_SUCCESS;
			}
			public dtStatus queryPolygons(float* center, float* halfExtents, dtQueryFilter* filter, dtPolyQuery* query)
			{
				dtAssert(m_nav);
				if (null == center || !dtVisfinite(center) ||
				null == halfExtents || !dtVisfinite(halfExtents) ||
				null == filter || null == query)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				float* bmin = stackalloc float[3], bmax = stackalloc float[3];
				dtVsub(bmin, center, halfExtents);
				dtVadd(bmax, center, halfExtents);
				int minx = 0, miny = 0, maxx = 0, maxy = 0;
				m_nav->calcTileLoc(bmin, ref minx, ref miny);
				m_nav->calcTileLoc(bmax, ref maxx, ref maxy);
				int MAX_NEIS = 32;
				dtMeshTile** neis = stackalloc dtMeshTile*[MAX_NEIS];
				for (int y = miny; y <= maxy; ++y)
				{
					for (int x = minx; x <= maxx; ++x)
					{
						int nneis = m_nav->getTilesAt(x, y, neis, MAX_NEIS);
						for (int j = 0; j < nneis; ++j)
						{
							queryPolygonsInTile(neis[j], bmin, bmax, filter, query);
						}
					}
				}
				return DT_SUCCESS;
			}
			public dtStatus findLocalNeighbourhood(dtPolyRef startRef, float* centerPos, float radius, dtQueryFilter* filter, dtPolyRef* resultRef, dtPolyRef* resultParent, int* resultCount, int maxResult)
			{
				dtAssert(m_nav);
				dtAssert(m_tinyNodePool);
				if (null == resultCount)
					return DT_FAILURE | DT_INVALID_PARAM;
				*resultCount = 0;
				if (!m_nav->isValidPolyRef(startRef) ||
				null == centerPos || !dtVisfinite(centerPos) ||
				radius < 0 || !dtMathIsfinite(radius) ||
				null == filter || maxResult < 0)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				int MAX_STACK = 48;
				dtNode* stack = stackalloc dtNode[MAX_STACK];
				int nstack = 0;
				m_tinyNodePool->clear();
				dtNode* startNode = m_tinyNodePool->getNode(startRef);
				startNode->pidx = 0;
				startNode->id = startRef;
				startNode->flags = (uint)dtNodeFlags.DT_NODE_CLOSED;
				stack[nstack++] = *startNode;
				float radiusSqr = dtSqr(radius);
				float* pa = stackalloc float[DT_VERTS_PER_POLYGON * 3];
				float* pb = stackalloc float[DT_VERTS_PER_POLYGON * 3];
				dtStatus status = DT_SUCCESS;
				int n = 0;
				if (n < maxResult)
				{
					resultRef[n] = startNode->id;
					if (null != resultParent)
						resultParent[n] = 0;
					++n;
				}
				else
				{
					status |= DT_BUFFER_TOO_SMALL;
				}
				float* va = stackalloc float[3], vb = stackalloc float[3];
				while (0 != nstack)
				{
					dtNode* curNode = stack;//stack[0];
					for (int i = 0; i < nstack - 1; ++i)
						stack[i] = stack[i + 1];
					nstack--;
					dtPolyRef curRef = curNode->id;
					dtMeshTile* curTile = (dtMeshTile*)0;
					dtPoly* curPoly = (dtPoly*)0;
					m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);
					for (uint i = curPoly->firstLink; i != DT_NULL_LINK; i = curTile->links[i].next)
					{
						dtLink* link = &curTile->links[i];
						dtPolyRef neighbourRef = link->Ref;
						if (0 == neighbourRef)
							continue;
						dtNode* neighbourNode = m_tinyNodePool->getNode(neighbourRef);
						if (null == neighbourNode)
							continue;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_CLOSED))
							continue;
						dtMeshTile* neighbourTile = (dtMeshTile*)0;
						dtPoly* neighbourPoly = (dtPoly*)0;
						m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
						if (neighbourPoly->getType() == (int)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
							continue;
						if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
							continue;
						//float* va = stackalloc float[3], vb = stackalloc float[3];
						memset(va, 0, sizeof(float) * 3);
						memset(vb, 0, sizeof(float) * 3);
						if (0 == getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
							continue;
						float tseg = 0f;
						float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, ref tseg);
						if (distSqr > radiusSqr)
							continue;
						neighbourNode->flags |= (uint)dtNodeFlags.DT_NODE_CLOSED;
						neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
						int npa = neighbourPoly->vertCount;
						for (int k = 0; k < npa; ++k)
							dtVcopy(&pa[k * 3], &neighbourTile->verts[neighbourPoly->verts[k] * 3]);
						bool overlap = false;
						for (int j = 0; j < n; ++j)
						{
							dtPolyRef pastRef = resultRef[j];
							bool connected = false;
							for (uint k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
							{
								if (curTile->links[k].Ref == pastRef)
								{
									connected = true;
									break;
								}
							}
							if (connected)
								continue;
							dtMeshTile* pastTile = (dtMeshTile*)0;
							dtPoly* pastPoly = (dtPoly*)0;
							m_nav->getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly);
							int npb = pastPoly->vertCount;
							for (int k = 0; k < npb; ++k)
								dtVcopy(&pb[k * 3], &pastTile->verts[pastPoly->verts[k] * 3]);
							if (dtOverlapPolyPoly2D(pa, npa, pb, npb))
							{
								overlap = true;
								break;
							}
						}
						if (overlap)
							continue;
						if (n < maxResult)
						{
							resultRef[n] = neighbourRef;
							if (null != resultParent)
								resultParent[n] = curRef;
							++n;
						}
						else
						{
							status |= DT_BUFFER_TOO_SMALL;
						}
						if (nstack < MAX_STACK)
						{
							stack[nstack++] = *neighbourNode;
						}
					}
				}
				*resultCount = n;
				return status;
			}
			public dtStatus moveAlongSurface(dtPolyRef startRef, float* startPos, float* endPos, dtQueryFilter* filter, float* resultPos, dtPolyRef* visited, int* visitedCount, int maxVisitedSize)
			{
				dtAssert(m_nav);
				dtAssert(m_tinyNodePool);
				if (null == visitedCount)
					return DT_FAILURE | DT_INVALID_PARAM;
				*visitedCount = 0;
				if (!m_nav->isValidPolyRef(startRef) ||
				null == startPos || !dtVisfinite(startPos) ||
				null == endPos || !dtVisfinite(endPos) ||
				null == filter || null == resultPos || null == visited ||
				maxVisitedSize <= 0)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				dtStatus status = DT_SUCCESS;
				int MAX_STACK = 48;
				dtNode** stack = stackalloc dtNode*[MAX_STACK];
				int nstack = 0;
				m_tinyNodePool->clear();
				dtNode* startNode = m_tinyNodePool->getNode(startRef);
				startNode->pidx = 0;
				startNode->cost = 0;
				startNode->total = 0;
				startNode->id = startRef;
				startNode->flags = (uint)dtNodeFlags.DT_NODE_CLOSED;
				stack[nstack++] = startNode;
				float* bestPos = stackalloc float[3];
				float bestDist = FLT_MAX;
				dtNode* bestNode = (dtNode*)0;
				dtVcopy(bestPos, startPos);
				float* searchPos = stackalloc float[3];
				dtVlerp(searchPos, startPos, endPos, 0.5f);
				float searchRadSqr = dtSqr(dtVdist(startPos, endPos) / 2.0f + 0.001f);
				float* verts = stackalloc float[DT_VERTS_PER_POLYGON * 3];
				int MAX_NEIS = 8;
				dtPolyRef* neis = stackalloc dtPolyRef[MAX_NEIS];
				while (0 != nstack)
				{
					dtNode* curNode = stack[0];
					for (int i = 0; i < nstack - 1; ++i)
						stack[i] = stack[i + 1];
					nstack--;
					dtPolyRef curRef = curNode->id;
					dtMeshTile* curTile = (dtMeshTile*)0;
					dtPoly* curPoly = (dtPoly*)0;
					m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);
					int nverts = curPoly->vertCount;
					for (int i = 0; i < nverts; ++i)
						dtVcopy(&verts[i * 3], &curTile->verts[curPoly->verts[i] * 3]);
					if (dtPointInPolygon(endPos, verts, nverts))
					{
						bestNode = curNode;
						dtVcopy(bestPos, endPos);
						break;
					}
					for (int i = 0, j = curPoly->vertCount - 1; i < curPoly->vertCount; j = i++)
					{
						//int MAX_NEIS = 8;
						int nneis = 0;
						//dtPolyRef* neis = stackalloc dtPolyRef[MAX_NEIS];
						memset(neis, 0, sizeof(dtPolyRef) * MAX_NEIS);
						if (0 != curPoly->neis[j] & 0 != DT_EXT_LINK)
						{
							for (uint k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
							{
								dtLink* link = &curTile->links[k];
								if (link->edge == j)
								{
									if (link->Ref != 0)
									{
										dtMeshTile* neiTile = (dtMeshTile*)0;
										dtPoly* neiPoly = (dtPoly*)0;
										m_nav->getTileAndPolyByRefUnsafe(link->Ref, &neiTile, &neiPoly);
										if (filter->passFilter(link->Ref, neiTile, neiPoly))
										{
											if (nneis < MAX_NEIS)
												neis[nneis++] = link->Ref;
										}
									}
								}
							}
						}
						else if (0 != curPoly->neis[j])
						{
							uint idx = (uint)(curPoly->neis[j] - 1);
							dtPolyRef Ref = m_nav->getPolyRefBase(curTile) | idx;
							if (filter->passFilter(Ref, curTile, &curTile->polys[idx]))
							{
								neis[nneis++] = Ref;
							}
						}
						if (0 == nneis)
						{
							float* vj = &verts[j * 3];
							float* vi = &verts[i * 3];
							float tseg = 0f;
							float distSqr = dtDistancePtSegSqr2D(endPos, vj, vi, ref tseg);
							if (distSqr < bestDist)
							{

								dtVlerp(bestPos, vj, vi, tseg);
								bestDist = distSqr;
								bestNode = curNode;
							}
						}
						else
						{
							for (int k = 0; k < nneis; ++k)
							{
								dtNode* neighbourNode = m_tinyNodePool->getNode(neis[k]);
								if (null == neighbourNode)
									continue;
								if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_CLOSED))
									continue;
								float* vj = &verts[j * 3];
								float* vi = &verts[i * 3];
								float tseg = 0f;
								float distSqr = dtDistancePtSegSqr2D(searchPos, vj, vi, ref tseg);
								if (distSqr > searchRadSqr)
									continue;
								if (nstack < MAX_STACK)
								{
									neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
									neighbourNode->flags |= (uint)dtNodeFlags.DT_NODE_CLOSED;
									stack[nstack++] = neighbourNode;
								}
							}
						}
					}
				}
				int n = 0;
				if (null != bestNode)
				{
					dtNode* prev = (dtNode*)0;
					dtNode* node = bestNode;
					do
					{
						dtNode* next = m_tinyNodePool->getNodeAtIdx(node->pidx);
						node->pidx = m_tinyNodePool->getNodeIdx(prev);
						prev = node;
						node = next;
					}
					while (null != node);
					node = prev;
					do
					{
						visited[n++] = node->id;
						if (n >= maxVisitedSize)
						{
							status |= DT_BUFFER_TOO_SMALL;
							break;
						}
						node = m_tinyNodePool->getNodeAtIdx(node->pidx);
					}
					while (null != node);
				}
				dtVcopy(resultPos, bestPos);
				*visitedCount = n;
				return status;
			}
			public dtStatus raycast(dtPolyRef startRef, float* startPos, float* endPos, dtQueryFilter* filter, float* t, float* hitNormal, dtPolyRef* path, int* pathCount, int maxPath)
			{
				dtRaycastHit hit;
				hit.path = path;
				hit.maxPath = maxPath;
				dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit);
				*t = hit.t;
				if (null != hitNormal)
					dtVcopy(hitNormal, hit.hitNormal);
				if (null != pathCount)
					*pathCount = hit.pathCount;
				return status;
			}
			public dtStatus raycast(dtPolyRef startRef, float* startPos, float* endPos, dtQueryFilter* filter, uint options, dtRaycastHit* hit, dtPolyRef prevRef = 0)
			{
				dtAssert(m_nav);
				if (null == hit)
					return DT_FAILURE | DT_INVALID_PARAM;
				hit->t = 0;
				hit->pathCount = 0;
				hit->pathCost = 0;
				if (!m_nav->isValidPolyRef(startRef) ||
				null == startPos || !dtVisfinite(startPos) ||
				null == endPos || !dtVisfinite(endPos) ||
				null == filter ||
				(0 != prevRef && !m_nav->isValidPolyRef(prevRef)))
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				float* dir = stackalloc float[3], curPos = stackalloc float[3], lastPos = stackalloc float[3];
				float* verts = stackalloc float[DT_VERTS_PER_POLYGON * 3 + 3];
				int n = 0;
				dtVcopy(curPos, startPos);
				dtVsub(dir, endPos, startPos);
				dtVset(hit->hitNormal, 0, 0, 0);
				dtStatus status = DT_SUCCESS;
				dtMeshTile* prevTile, tile, nextTile;
				dtPoly* prevPoly, poly, nextPoly;
				dtPolyRef curRef;
				curRef = startRef;
				tile = (dtMeshTile*)0;
				poly = (dtPoly*)0;
				m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly);
				nextTile = prevTile = tile;
				nextPoly = prevPoly = poly;
				if (0 != prevRef)
					m_nav->getTileAndPolyByRefUnsafe(prevRef, &prevTile, &prevPoly);
				float* eDir = stackalloc float[3], diff = stackalloc float[3];
				while (0 != curRef)
				{
					int nv = 0;
					for (int i = 0; i < poly->vertCount; ++i)
					{
						dtVcopy(&verts[nv * 3], &tile->verts[poly->verts[i] * 3]);
						nv++;
					}
					float tmin = 0f, tmax = 0f;
					int segMin = 0, segMax = 0;
					if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, ref tmin, ref tmax, ref segMin, ref segMax))
					{
						hit->pathCount = n;
						return status;
					}
					hit->hitEdgeIndex = segMax;
					if (tmax > hit->t)
						hit->t = tmax;
					if (n < hit->maxPath)
						hit->path[n++] = curRef;
					else
						status |= DT_BUFFER_TOO_SMALL;
					if (segMax == -1)
					{
						hit->t = FLT_MAX;
						hit->pathCount = n;
						if (0 != (options & (uint)dtRaycastOptions.DT_RAYCAST_USE_COSTS))
							hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly);
						return status;
					}
					dtPolyRef nextRef = 0;
					for (uint i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
					{
						dtLink* link = &tile->links[i];
						if ((int)link->edge != segMax)
							continue;
						nextTile = (dtMeshTile*)0;
						nextPoly = (dtPoly*)0;
						m_nav->getTileAndPolyByRefUnsafe(link->Ref, &nextTile, &nextPoly);
						if (nextPoly->getType() == (int)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
							continue;
						if (!filter->passFilter(link->Ref, nextTile, nextPoly))
							continue;
						if (link->side == 0xff)
						{
							nextRef = link->Ref;
							break;
						}
						if (link->bmin == 0 && link->bmax == 255)
						{
							nextRef = link->Ref;
							break;
						}
						int v0 = poly->verts[link->edge];
						int v1 = poly->verts[(link->edge + 1) % poly->vertCount];
						float* left = &tile->verts[v0 * 3];
						float* right = &tile->verts[v1 * 3];
						if (link->side == 0 || link->side == 4)
						{
							float s = 1.0f / 255.0f;
							float lmin = left[2] + (right[2] - left[2]) * (link->bmin * s);
							float lmax = left[2] + (right[2] - left[2]) * (link->bmax * s);
							if (lmin > lmax) dtSwap(ref lmin, ref lmax);
							float z = startPos[2] + (endPos[2] - startPos[2]) * tmax;
							if (z >= lmin && z <= lmax)
							{
								nextRef = link->Ref;
								break;
							}
						}
						else if (link->side == 2 || link->side == 6)
						{
							float s = 1.0f / 255.0f;
							float lmin = left[0] + (right[0] - left[0]) * (link->bmin * s);
							float lmax = left[0] + (right[0] - left[0]) * (link->bmax * s);
							if (lmin > lmax) dtSwap(ref lmin, ref lmax);
							float x = startPos[0] + (endPos[0] - startPos[0]) * tmax;
							if (x >= lmin && x <= lmax)
							{
								nextRef = link->Ref;
								break;
							}
						}
					}
					if (0 != (options & (uint)dtRaycastOptions.DT_RAYCAST_USE_COSTS))
					{
						dtVcopy(lastPos, curPos);
						dtVmad(curPos, startPos, dir, hit->t);
						float* e1 = &verts[segMax * 3];
						float* e2 = &verts[((segMax + 1) % nv) * 3];
						//float* eDir = stackalloc float[3], diff = stackalloc float[3];
						memset(eDir, 0, sizeof(float) * 3);
						memset(diff, 0, sizeof(float) * 3);
						dtVsub(eDir, e2, e1);
						dtVsub(diff, curPos, e1);
						float s = dtSqr(eDir[0]) > dtSqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2];
						curPos[1] = e1[1] + eDir[1] * s;
						hit->pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly);
					}
					if (0 == nextRef)
					{
						int a = segMax;
						int b = segMax + 1 < nv ? segMax + 1 : 0;
						float* va = &verts[a * 3];
						float* vb = &verts[b * 3];
						float dx = vb[0] - va[0];
						float dz = vb[2] - va[2];
						hit->hitNormal[0] = dz;
						hit->hitNormal[1] = 0;
						hit->hitNormal[2] = -dx;
						dtVnormalize(hit->hitNormal);
						hit->pathCount = n;
						return status;
					}
					prevRef = curRef;
					curRef = nextRef;
					prevTile = tile;
					tile = nextTile;
					prevPoly = poly;
					poly = nextPoly;
				}
				hit->pathCount = n;
				return status;
			}
			public dtStatus findDistanceToWall(dtPolyRef startRef, float* centerPos, float maxRadius, dtQueryFilter* filter, float* hitDist, float* hitPos, float* hitNormal)
			{
				dtAssert(m_nav);
				dtAssert(m_nodePool);
				dtAssert(m_openList);
				if (!m_nav->isValidPolyRef(startRef) ||
				null == centerPos || !dtVisfinite(centerPos) ||
				maxRadius < 0 || !dtMathIsfinite(maxRadius) ||
				null == filter || null == hitDist || null == hitPos || null == hitNormal)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				m_nodePool->clear();
				m_openList->clear();
				dtNode* startNode = m_nodePool->getNode(startRef);
				dtVcopy(startNode->pos, centerPos);
				startNode->pidx = 0;
				startNode->cost = 0;
				startNode->total = 0;
				startNode->id = startRef;
				startNode->flags = (uint)dtNodeFlags.DT_NODE_OPEN;
				m_openList->push(startNode);
				float radiusSqr = dtSqr(maxRadius);
				dtStatus status = DT_SUCCESS;
				while (!m_openList->empty())
				{
					dtNode* bestNode = m_openList->pop();
					bestNode->flags &= ~(uint)dtNodeFlags.DT_NODE_OPEN;
					bestNode->flags |= (uint)dtNodeFlags.DT_NODE_CLOSED;
					dtPolyRef bestRef = bestNode->id;
					dtMeshTile* bestTile = (dtMeshTile*)0;
					dtPoly* bestPoly = (dtPoly*)0;
					m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
					dtPolyRef parentRef = 0;
					dtMeshTile* parentTile = (dtMeshTile*)0;
					dtPoly* parentPoly = (dtPoly*)0;
					if (0 != bestNode->pidx)
						parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
					if (0 != parentRef)
						m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
					for (int i = 0, j = (int)bestPoly->vertCount - 1; i < (int)bestPoly->vertCount; j = i++)
					{
						if (0 != bestPoly->neis[j] & 0 != DT_EXT_LINK)
						{
							bool solid = true;
							for (uint k = bestPoly->firstLink; k != DT_NULL_LINK; k = bestTile->links[k].next)
							{
								dtLink* link = &bestTile->links[k];
								if (link->edge == j)
								{
									if (link->Ref != 0)
									{
										dtMeshTile* neiTile = (dtMeshTile*)0;
										dtPoly* neiPoly = (dtPoly*)0;
										m_nav->getTileAndPolyByRefUnsafe(link->Ref, &neiTile, &neiPoly);
										if (filter->passFilter(link->Ref, neiTile, neiPoly))
											solid = false;
									}
									break;
								}
							}
							if (!solid) continue;
						}
						else if (0 != bestPoly->neis[j])
						{
							uint idx = (uint)(bestPoly->neis[j] - 1);
							dtPolyRef Ref = m_nav->getPolyRefBase(bestTile) | idx;
							if (filter->passFilter(Ref, bestTile, &bestTile->polys[idx]))
								continue;
						}
						float* vj = &bestTile->verts[bestPoly->verts[j] * 3];
						float* vi = &bestTile->verts[bestPoly->verts[i] * 3];
						float tseg = 0;
						float distSqr = dtDistancePtSegSqr2D(centerPos, vj, vi, ref tseg);
						if (distSqr > radiusSqr)
							continue;
						radiusSqr = distSqr;
						hitPos[0] = vj[0] + (vi[0] - vj[0]) * tseg;
						hitPos[1] = vj[1] + (vi[1] - vj[1]) * tseg;
						hitPos[2] = vj[2] + (vi[2] - vj[2]) * tseg;
					}
					for (uint i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
					{
						dtLink* link = &bestTile->links[i];
						dtPolyRef neighbourRef = link->Ref;
						if (0 == neighbourRef || neighbourRef == parentRef)
							continue;
						dtMeshTile* neighbourTile = (dtMeshTile*)0;
						dtPoly* neighbourPoly = (dtPoly*)0;
						m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
						if (neighbourPoly->getType() == (int)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
							continue;
						float* va = &bestTile->verts[bestPoly->verts[link->edge] * 3];
						float* vb = &bestTile->verts[bestPoly->verts[(link->edge + 1) % bestPoly->vertCount] * 3];
						float tseg = 0f;
						float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, ref tseg);
						if (distSqr > radiusSqr)
							continue;
						if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
							continue;
						dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
						if (null == neighbourNode)
						{
							status |= DT_OUT_OF_NODES;
							continue;
						}
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_CLOSED))
							continue;
						if (neighbourNode->flags == 0)
						{
							getEdgeMidPoint(bestRef, bestPoly, bestTile,
							neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos);
						}
						float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN) && total >= neighbourNode->total)
							continue;
						neighbourNode->id = neighbourRef;
						neighbourNode->flags = (neighbourNode->flags & ~(uint)dtNodeFlags.DT_NODE_CLOSED);
						neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
						neighbourNode->total = total;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN))
						{
							m_openList->modify(neighbourNode);
						}
						else
						{
							neighbourNode->flags |= (uint)dtNodeFlags.DT_NODE_OPEN;
							m_openList->push(neighbourNode);
						}
					}
				}
				dtVsub(hitNormal, centerPos, hitPos);
				dtVnormalize(hitNormal);
				*hitDist = dtMathSqrtf(radiusSqr);
				return status;
			}
			public dtStatus getPolyWallSegments(dtPolyRef Ref, dtQueryFilter* filter, float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, int maxSegments)
			{
				dtAssert(m_nav);
				if (null == segmentCount)
					return DT_FAILURE | DT_INVALID_PARAM;
				*segmentCount = 0;
				dtMeshTile* tile = (dtMeshTile*)0;
				dtPoly* poly = (dtPoly*)0;
				if (dtStatusFailed(m_nav->getTileAndPolyByRef(Ref, &tile, &poly)))
					return DT_FAILURE | DT_INVALID_PARAM;
				if (null == filter || null == segmentVerts || maxSegments < 0)
					return DT_FAILURE | DT_INVALID_PARAM;
				int n = 0;
				int MAX_INTERVAL = 16;
				dtSegInterval* ints = stackalloc dtSegInterval[MAX_INTERVAL];
				int nints;
				bool storePortals = segmentRefs != null;
				dtStatus status = DT_SUCCESS;
				for (int i = 0, j = (int)poly->vertCount - 1; i < (int)poly->vertCount; j = i++)
				{
					nints = 0;
					if (0 != poly->neis[j] & 0 != DT_EXT_LINK)
					{
						for (uint k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
						{
							dtLink* link = &tile->links[k];
							if (link->edge == j)
							{
								if (link->Ref != 0)
								{
									dtMeshTile* neiTile = (dtMeshTile*)0;
									dtPoly* neiPoly = (dtPoly*)0;
									m_nav->getTileAndPolyByRefUnsafe(link->Ref, &neiTile, &neiPoly);
									if (filter->passFilter(link->Ref, neiTile, neiPoly))
									{
										insertInterval(ints, ref nints, MAX_INTERVAL, link->bmin, link->bmax, link->Ref);
									}
								}
							}
						}
					}
					else
					{
						dtPolyRef neiRef = 0;
						if (0 != poly->neis[j])
						{
							uint idx = (uint)(poly->neis[j] - 1);
							neiRef = m_nav->getPolyRefBase(tile) | idx;
							if (!filter->passFilter(neiRef, tile, &tile->polys[idx]))
								neiRef = 0;
						}
						if (neiRef != 0 && !storePortals)
							continue;
						if (n < maxSegments)
						{
							float* vj1 = &tile->verts[poly->verts[j] * 3];
							float* vi1 = &tile->verts[poly->verts[i] * 3];
							float* seg = &segmentVerts[n * 6];
							dtVcopy(seg + 0, vj1);
							dtVcopy(seg + 3, vi1);
							if (null != segmentRefs)
								segmentRefs[n] = neiRef;
							n++;
						}
						else
						{
							status |= DT_BUFFER_TOO_SMALL;
						}
						continue;
					}
					insertInterval(ints, ref nints, MAX_INTERVAL, -1, 0, 0);
					insertInterval(ints, ref nints, MAX_INTERVAL, 255, 256, 0);
					float* vj = &tile->verts[poly->verts[j] * 3];
					float* vi = &tile->verts[poly->verts[i] * 3];
					for (int k = 1; k < nints; ++k)
					{
						if (storePortals && 0 != ints[k].Ref)
						{
							float tmin = ints[k].tmin / 255.0f;
							float tmax = ints[k].tmax / 255.0f;
							if (n < maxSegments)
							{
								float* seg = &segmentVerts[n * 6];
								dtVlerp(seg + 0, vj, vi, tmin);
								dtVlerp(seg + 3, vj, vi, tmax);
								if (null != segmentRefs)
									segmentRefs[n] = ints[k].Ref;
								n++;
							}
							else
							{
								status |= DT_BUFFER_TOO_SMALL;
							}
						}
						int imin = ints[k - 1].tmax;
						int imax = ints[k].tmin;
						if (imin != imax)
						{
							float tmin = imin / 255.0f;
							float tmax = imax / 255.0f;
							if (n < maxSegments)
							{
								float* seg = &segmentVerts[n * 6];
								dtVlerp(seg + 0, vj, vi, tmin);
								dtVlerp(seg + 3, vj, vi, tmax);
								if (null != segmentRefs)
									segmentRefs[n] = 0;
								n++;
							}
							else
							{
								status |= DT_BUFFER_TOO_SMALL;
							}
						}
					}
				}
				*segmentCount = n;
				return status;
			}
			public dtStatus findRandomPoint(dtQueryFilter* filter, Func<float> frand, dtPolyRef* randomRef, float* randomPt)
			{
				dtAssert(m_nav);
				if (null == filter || null == frand || null == randomRef || null == randomPt)
					return DT_FAILURE | DT_INVALID_PARAM;
				dtMeshTile* tile = (dtMeshTile*)0;
				float tsum = 0.0f;
				for (int i = 0; i < m_nav->getMaxTiles(); i++)
				{
					dtMeshTile* t1 = m_nav->getTile(i);
					if (null == t1 || null == t1->header) continue;
					float area = 1.0f;
					tsum += area;
					float u = frand();
					if (u * tsum <= area)
						tile = t1;
				}
				if (null == tile)
					return DT_FAILURE;
				dtPoly* poly = (dtPoly*)0;
				dtPolyRef polyRef = 0;
				dtPolyRef @base = m_nav->getPolyRefBase(tile);
				float areaSum = 0.0f;
				for (int i = 0; i < tile->header->polyCount; ++i)
				{
					dtPoly* p = &tile->polys[i];
					if (p->getType() != (int)dtPolyTypes.DT_POLYTYPE_GROUND)
						continue;
					dtPolyRef Ref = @base | (dtPolyRef)i;
					if (!filter->passFilter(Ref, tile, p))
						continue;
					float polyArea = 0.0f;
					for (int j = 2; j < p->vertCount; ++j)
					{
						float* va = &tile->verts[p->verts[0] * 3];
						float* vb = &tile->verts[p->verts[j - 1] * 3];
						float* vc = &tile->verts[p->verts[j] * 3];
						polyArea += dtTriArea2D(va, vb, vc);
					}
					areaSum += polyArea;
					float u = frand();
					if (u * areaSum <= polyArea)
					{
						poly = p;
						polyRef = Ref;
					}
				}
				if (null == poly)
					return DT_FAILURE;
				float* v = &tile->verts[poly->verts[0] * 3];
				float* verts = stackalloc float[3 * DT_VERTS_PER_POLYGON];
				float* areas = stackalloc float[DT_VERTS_PER_POLYGON];
				dtVcopy(&verts[0 * 3], v);
				for (int j = 1; j < poly->vertCount; ++j)
				{
					v = &tile->verts[poly->verts[j] * 3];
					dtVcopy(&verts[j * 3], v);
				}
				float s = frand();
				float t = frand();
				float* pt = stackalloc float[3];
				dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt);
				closestPointOnPoly(polyRef, pt, pt, null);
				dtVcopy(randomPt, pt);
				*randomRef = polyRef;
				return DT_SUCCESS;
			}
			public dtStatus findRandomPointAroundCircle(//dtPolyRef startRef, float* centerPos, float maxRadius, dtQueryFilter* filter, Func<float> frand)
				dtPolyRef startRef, float* centerPos, float maxRadius, dtQueryFilter* filter, Func<float> frand, dtPolyRef* randomRef, float* randomPt)
			{
				dtAssert(m_nav);
				dtAssert(m_nodePool);
				dtAssert(m_openList);
				if (!m_nav->isValidPolyRef(startRef) ||
				null == centerPos || !dtVisfinite(centerPos) ||
				maxRadius < 0 || !dtMathIsfinite(maxRadius) ||
				null == filter || null == frand || null == randomRef || null == randomPt)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				dtMeshTile* startTile = (dtMeshTile*)0;
				dtPoly* startPoly = (dtPoly*)0;
				m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly);
				if (!filter->passFilter(startRef, startTile, startPoly))
					return DT_FAILURE | DT_INVALID_PARAM;
				m_nodePool->clear();
				m_openList->clear();
				dtNode* startNode = m_nodePool->getNode(startRef);
				dtVcopy(startNode->pos, centerPos);
				startNode->pidx = 0;
				startNode->cost = 0;
				startNode->total = 0;
				startNode->id = startRef;
				startNode->flags = (uint)dtNodeFlags.DT_NODE_OPEN;
				m_openList->push(startNode);
				dtStatus status = DT_SUCCESS;
				float radiusSqr = dtSqr(maxRadius);
				float areaSum = 0.0f;
				dtMeshTile* randomTile = (dtMeshTile*)0;
				dtPoly* randomPoly = (dtPoly*)0;
				dtPolyRef randomPolyRef = 0;
				float* vax = stackalloc float[3], vbx = stackalloc float[3];
				while (!m_openList->empty())
				{
					dtNode* bestNode = m_openList->pop();
					bestNode->flags &= ~(uint)dtNodeFlags.DT_NODE_OPEN;
					bestNode->flags |= (uint)dtNodeFlags.DT_NODE_CLOSED;
					dtPolyRef bestRef = bestNode->id;
					dtMeshTile* bestTile = (dtMeshTile*)0;
					dtPoly* bestPoly = (dtPoly*)0;
					m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
					if (bestPoly->getType() == (int)dtPolyTypes.DT_POLYTYPE_GROUND)
					{
						float polyArea = 0.0f;
						for (int j = 2; j < bestPoly->vertCount; ++j)
						{
							float* va = &bestTile->verts[bestPoly->verts[0] * 3];
							float* vb = &bestTile->verts[bestPoly->verts[j - 1] * 3];
							float* vc = &bestTile->verts[bestPoly->verts[j] * 3];
							polyArea += dtTriArea2D(va, vb, vc);
						}
						areaSum += polyArea;
						float u = frand();
						if (u * areaSum <= polyArea)
						{
							randomTile = bestTile;
							randomPoly = bestPoly;
							randomPolyRef = bestRef;
						}
					}
					dtPolyRef parentRef = 0;
					dtMeshTile* parentTile = (dtMeshTile*)0;
					dtPoly* parentPoly = (dtPoly*)0;
					if (0 != bestNode->pidx)
						parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
					if (0 != parentRef)
						m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
					for (uint i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
					{
						dtLink* link = &bestTile->links[i];
						dtPolyRef neighbourRef = link->Ref;
						if (0 == neighbourRef || neighbourRef == parentRef)
							continue;
						dtMeshTile* neighbourTile = (dtMeshTile*)0;
						dtPoly* neighbourPoly = (dtPoly*)0;
						m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
						if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
							continue;
						//float* vax = stackalloc float[3], vbx = stackalloc float[3];
						memset(vax, 0, sizeof(float) * 3);
						memset(vbx, 0, sizeof(float) * 3);
						if (0 == getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, vax, vbx))
							continue;
						float tseg = 0f;
						float distSqr = dtDistancePtSegSqr2D(centerPos, vax, vbx, ref tseg);
						if (distSqr > radiusSqr)
							continue;
						dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
						if (null == neighbourNode)
						{
							status |= DT_OUT_OF_NODES;
							continue;
						}
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_CLOSED))
							continue;
						if (neighbourNode->flags == 0)
							dtVlerp(neighbourNode->pos, vax, vbx, 0.5f);
						float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN) && total >= neighbourNode->total)
							continue;
						neighbourNode->id = neighbourRef;
						neighbourNode->flags = (neighbourNode->flags & ~(uint)dtNodeFlags.DT_NODE_CLOSED);
						neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
						neighbourNode->total = total;
						if (0 != (neighbourNode->flags & (uint)dtNodeFlags.DT_NODE_OPEN))
						{
							m_openList->modify(neighbourNode);
						}
						else
						{
							neighbourNode->flags = (uint)dtNodeFlags.DT_NODE_OPEN;
							m_openList->push(neighbourNode);
						}
					}
				}
				if (null == randomPoly)
					return DT_FAILURE;
				float* v = &randomTile->verts[randomPoly->verts[0] * 3];
				float* verts = stackalloc float[3 * DT_VERTS_PER_POLYGON];
				float* areas = stackalloc float[DT_VERTS_PER_POLYGON];
				dtVcopy(&verts[0 * 3], v);
				for (int j = 1; j < randomPoly->vertCount; ++j)
				{
					v = &randomTile->verts[randomPoly->verts[j] * 3];
					dtVcopy(&verts[j * 3], v);
				}
				float s = frand();
				float t = frand();
				float* pt = stackalloc float[3];
				dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt);
				closestPointOnPoly(randomPolyRef, pt, pt, null);
				dtVcopy(randomPt, pt);
				*randomRef = randomPolyRef;
				return status;
			}
			public dtStatus closestPointOnPoly(dtPolyRef Ref, float* pos, float* closest, bool* posOverPoly)
			{
				dtAssert(m_nav);
				if (!m_nav->isValidPolyRef(Ref) ||
				null == pos || !dtVisfinite(pos) ||
				null == closest)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				m_nav->closestPointOnPoly(Ref, pos, closest, posOverPoly);
				return DT_SUCCESS;
			}
			public dtStatus closestPointOnPolyBoundary(dtPolyRef Ref, float* pos, float* closest)
			{
				dtAssert(m_nav);
				dtMeshTile* tile = (dtMeshTile*)0;
				dtPoly* poly = (dtPoly*)0;
				if (dtStatusFailed(m_nav->getTileAndPolyByRef(Ref, &tile, &poly)))
					return DT_FAILURE | DT_INVALID_PARAM;
				if (null == pos || !dtVisfinite(pos) || null == closest)
					return DT_FAILURE | DT_INVALID_PARAM;
				float* verts = stackalloc float[DT_VERTS_PER_POLYGON * 3];
				float* edged = stackalloc float[DT_VERTS_PER_POLYGON];
				float* edget = stackalloc float[DT_VERTS_PER_POLYGON];
				int nv = 0;
				for (int i = 0; i < poly->vertCount; ++i)
				{
					dtVcopy(&verts[nv * 3], &tile->verts[poly->verts[i] * 3]);
					nv++;
				}
				bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
				if (inside)
				{
					dtVcopy(closest, pos);
				}
				else
				{
					float dmin = edged[0];
					int imin = 0;
					for (int i = 1; i < nv; ++i)
					{
						if (edged[i] < dmin)
						{
							dmin = edged[i];
							imin = i;
						}
					}
					float* va = &verts[imin * 3];
					float* vb = &verts[((imin + 1) % nv) * 3];
					dtVlerp(closest, va, vb, edget[imin]);
				}
				return DT_SUCCESS;
			}
			public dtStatus getPolyHeight(dtPolyRef Ref, float* pos, float* height)
			{
				dtAssert(m_nav);
				dtMeshTile* tile = (dtMeshTile*)0;
				dtPoly* poly = (dtPoly*)0;
				if (dtStatusFailed(m_nav->getTileAndPolyByRef(Ref, &tile, &poly)))
					return DT_FAILURE | DT_INVALID_PARAM;
				if (null == pos || !dtVisfinite2D(pos))
					return DT_FAILURE | DT_INVALID_PARAM;
				if (poly->getType() == (int)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
				{
					float* v0 = &tile->verts[poly->verts[0] * 3];
					float* v1 = &tile->verts[poly->verts[1] * 3];
					float t = 0f;
					dtDistancePtSegSqr2D(pos, v0, v1, ref t);
					if (null != height)
						*height = v0[1] + (v1[1] - v0[1]) * t;
					return DT_SUCCESS;
				}
				return m_nav->getPolyHeight(tile, poly, pos, height)
				? DT_SUCCESS
				: DT_FAILURE | DT_INVALID_PARAM;
			}
			public bool isValidPolyRef(dtPolyRef Ref, dtQueryFilter* filter)
			{
				dtMeshTile* tile = (dtMeshTile*)0;
				dtPoly* poly = (dtPoly*)0;
				dtStatus status = m_nav->getTileAndPolyByRef(Ref, &tile, &poly);
				if (dtStatusFailed(status))
					return false;
				if (!filter->passFilter(Ref, tile, poly))
					return false;
				return true;
			}
			public bool isInClosedList(dtPolyRef Ref)
			{
				if (null == m_nodePool) return false;
				dtNode* nodes = stackalloc dtNode[DT_MAX_STATES_PER_NODE];
				int n = (int)m_nodePool->findNodes(Ref, nodes, DT_MAX_STATES_PER_NODE);
				for (int i = 0; i < n; i++)
				{
					if (0 != (nodes[i].flags & (uint)dtNodeFlags.DT_NODE_CLOSED))
						return true;
				}
				return false;
			}
			public dtNodePool* getNodePool()
			{
				return m_nodePool;
			}
			public dtNavMesh* getAttachedNavMesh()
			{
				return m_nav;
			}
			public void queryPolygonsInTile(dtMeshTile* tile, float* qmin, float* qmax, dtQueryFilter* filter, dtPolyQuery* query)
			{
				dtAssert(m_nav);
				int batchSize = 32;
				dtPolyRef* polyRefs = stackalloc dtPolyRef[batchSize];
				dtPoly* polys = stackalloc dtPoly[batchSize];
				int n = 0;
				if (null != tile->bvTree)
				{
					dtBVNode* node = &tile->bvTree[0];
					dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount];
					float* tbmin = tile->header->bmin;
					float* tbmax = tile->header->bmax;
					float qfac = tile->header->bvQuantFactor;
					ushort* bmin = stackalloc ushort[3], bmax = stackalloc ushort[3];
					float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
					float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
					float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
					float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
					float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
					float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
					bmin[0] = (ushort)((ushort)(qfac * minx) & 0xfffe);
					bmin[1] = (ushort)((ushort)(qfac * miny) & 0xfffe);
					bmin[2] = (ushort)((ushort)(qfac * minz) & 0xfffe);
					bmax[0] = (ushort)((ushort)(qfac * maxx + 1) | 1);
					bmax[1] = (ushort)((ushort)(qfac * maxy + 1) | 1);
					bmax[2] = (ushort)((ushort)(qfac * maxz + 1) | 1);
					dtPolyRef @base = m_nav->getPolyRefBase(tile);
					while (node < end)
					{
						bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax);
						bool isLeafNode = node->i >= 0;
						if (isLeafNode && overlap)
						{
							dtPolyRef Ref = @base | (dtPolyRef)node->i;
							if (filter->passFilter(Ref, tile, &tile->polys[node->i]))
							{
								polyRefs[n] = Ref;
								polys[n] = tile->polys[node->i];
								if (n == batchSize - 1)
								{
									query->process(tile, polys, polyRefs, batchSize);
									n = 0;
								}
								else
								{
									n++;
								}
							}
						}
						if (overlap || isLeafNode)
							node++;
						else
						{
							int escapeIndex = -node->i;
							node += escapeIndex;
						}
					}
				}
				else
				{
					float* bmin = stackalloc float[3], bmax = stackalloc float[3];
					dtPolyRef @base = m_nav->getPolyRefBase(tile);
					for (int i = 0; i < tile->header->polyCount; ++i)
					{
						dtPoly* p = &tile->polys[i];
						if (p->getType() == (int)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
							continue;
						dtPolyRef Ref = @base | (dtPolyRef)i;
						if (!filter->passFilter(Ref, tile, p))
							continue;
						float* v = &tile->verts[p->verts[0] * 3];
						dtVcopy(bmin, v);
						dtVcopy(bmax, v);
						for (int j = 1; j < p->vertCount; ++j)
						{
							v = &tile->verts[p->verts[j] * 3];
							dtVmin(bmin, v);
							dtVmax(bmax, v);
						}
						if (dtOverlapBounds(qmin, qmax, bmin, bmax))
						{
							polyRefs[n] = Ref;
							polys[n] = *p;
							if (n == batchSize - 1)
							{
								query->process(tile, polys, polyRefs, batchSize);
								n = 0;
							}
							else
							{
								n++;
							}
						}
					}
				}
				if (n > 0)
					query->process(tile, polys, polyRefs, n);
			}
			public dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, ref byte fromType, ref byte toType)
			{
				dtAssert(m_nav);
				dtMeshTile* fromTile = (dtMeshTile*)0;
				dtPoly* fromPoly = (dtPoly*)0;
				if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
					return DT_FAILURE | DT_INVALID_PARAM;
				fromType = fromPoly->getType();
				dtMeshTile* toTile = (dtMeshTile*)0;
				dtPoly* toPoly = (dtPoly*)0;
				if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
					return DT_FAILURE | DT_INVALID_PARAM;
				toType = toPoly->getType();
				return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right);
			}
			public dtStatus getPortalPoints(dtPolyRef from, dtPoly* fromPoly, dtMeshTile* fromTile, dtPolyRef to, dtPoly* toPoly, dtMeshTile* toTile, float* left, float* right)
			{
				dtLink* link = (dtLink*)0;
				for (uint i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
				{
					if (fromTile->links[i].Ref == to)
					{
						link = &fromTile->links[i];
						break;
					}
				}
				if (null == link)
					return DT_FAILURE | DT_INVALID_PARAM;
				if (fromPoly->getType() == (int)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
				{
					for (uint i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
					{
						if (fromTile->links[i].Ref == to)
						{
							int v = fromTile->links[i].edge;
							dtVcopy(left, &fromTile->verts[fromPoly->verts[v] * 3]);
							dtVcopy(right, &fromTile->verts[fromPoly->verts[v] * 3]);
							return DT_SUCCESS;
						}
					}
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				if (toPoly->getType() == (int)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
				{
					for (uint i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next)
					{
						if (toTile->links[i].Ref == from)
						{
							int v = toTile->links[i].edge;
							dtVcopy(left, &toTile->verts[toPoly->verts[v] * 3]);
							dtVcopy(right, &toTile->verts[toPoly->verts[v] * 3]);
							return DT_SUCCESS;
						}
					}
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				int v0 = fromPoly->verts[link->edge];
				int v1 = fromPoly->verts[(link->edge + 1) % fromPoly->vertCount];
				dtVcopy(left, &fromTile->verts[v0 * 3]);
				dtVcopy(right, &fromTile->verts[v1 * 3]);
				if (link->side != 0xff)
				{
					if (link->bmin != 0 || link->bmax != 255)
					{
						float s = 1.0f / 255.0f;
						float tmin = link->bmin * s;
						float tmax = link->bmax * s;
						dtVlerp(left, &fromTile->verts[v0 * 3], &fromTile->verts[v1 * 3], tmin);
						dtVlerp(right, &fromTile->verts[v0 * 3], &fromTile->verts[v1 * 3], tmax);
					}
				}
				return DT_SUCCESS;
			}
			public dtStatus getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid)
			{
				float* left = stackalloc float[3], right = stackalloc float[3];
				byte fromType = 0, toType = 0;
				if (dtStatusFailed(getPortalPoints(from, to, left, right, ref fromType, ref toType)))
					return DT_FAILURE | DT_INVALID_PARAM;
				mid[0] = (left[0] + right[0]) * 0.5f;
				mid[1] = (left[1] + right[1]) * 0.5f;
				mid[2] = (left[2] + right[2]) * 0.5f;
				return DT_SUCCESS;
			}
			public dtStatus getEdgeMidPoint(dtPolyRef from, dtPoly* fromPoly, dtMeshTile* fromTile, dtPolyRef to, dtPoly* toPoly, dtMeshTile* toTile, float* mid)
			{
				float* left = stackalloc float[3], right = stackalloc float[3];
				if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
					return DT_FAILURE | DT_INVALID_PARAM;
				mid[0] = (left[0] + right[0]) * 0.5f;
				mid[1] = (left[1] + right[1]) * 0.5f;
				mid[2] = (left[2] + right[2]) * 0.5f;
				return DT_SUCCESS;
			}
			public dtStatus appendVertex(float* pos, byte flags, dtPolyRef Ref, float* straightPath, byte* straightPathFlags, dtPolyRef* straightPathRefs, ref int straightPathCount, int maxStraightPath)
			{
				if ((straightPathCount) > 0 && dtVequal(&straightPath[((straightPathCount) - 1) * 3], pos))
				{
					if (null != straightPathFlags)
						straightPathFlags[(straightPathCount) - 1] = flags;
					if (null != straightPathRefs)
						straightPathRefs[(straightPathCount) - 1] = Ref;
				}
				else
				{
					dtVcopy(&straightPath[(straightPathCount) * 3], pos);
					if (null != straightPathFlags)
						straightPathFlags[(straightPathCount)] = flags;
					if (null != straightPathRefs)
						straightPathRefs[(straightPathCount)] = Ref;
					(straightPathCount)++;
					if ((straightPathCount) >= maxStraightPath)
					{
						return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
					}
					if (flags == (byte)dtStraightPathFlags.DT_STRAIGHTPATH_END)
					{
						return DT_SUCCESS;
					}
				}
				return DT_IN_PROGRESS;
			}
			public dtStatus appendPortals(int startIdx, int endIdx, float* endPos, dtPolyRef* path, float* straightPath, byte* straightPathFlags, dtPolyRef* straightPathRefs, ref int straightPathCount, int maxStraightPath, int options)
			{
				float* startPos = &straightPath[(straightPathCount - 1) * 3];
				dtStatus stat = 0;
				float* left = stackalloc float[3], right = stackalloc float[3];
				float* pt = stackalloc float[3];
				for (int i = startIdx; i < endIdx; i++)
				{
					dtPolyRef from = path[i];
					dtMeshTile* fromTile = (dtMeshTile*)0;
					dtPoly* fromPoly = (dtPoly*)0;
					if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
						return DT_FAILURE | DT_INVALID_PARAM;
					dtPolyRef to = path[i + 1];
					dtMeshTile* toTile = (dtMeshTile*)0;
					dtPoly* toPoly = (dtPoly*)0;
					if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
						return DT_FAILURE | DT_INVALID_PARAM;
					//float* left = stackalloc float[3], right = stackalloc float[3];
					memset(left, 0, sizeof(float) * 3);
					memset(right, 0, sizeof(float) * 3);
					if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
						break;
					if (0 != (options & (int)dtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS))
					{
						if (fromPoly->getArea() == toPoly->getArea())
							continue;
					}
					float s = 0f, t = 0f;
					if (dtIntersectSegSeg2D(startPos, endPos, left, right, ref s, ref t))
					{
						//float* pt = stackalloc float[3];
						memset(pt, 0, sizeof(float) * 3);
						dtVlerp(pt, left, right, t);
						stat = appendVertex(pt, 0, path[i + 1],
						straightPath, straightPathFlags, straightPathRefs,
						ref straightPathCount, maxStraightPath);
						if (stat != DT_IN_PROGRESS)
							return stat;
					}
				}
				return DT_IN_PROGRESS;
			}
			public dtStatus getPathToNode(dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath)
			{
				dtNode* curNode = endNode;
				int length = 0;
				do
				{
					length++;
					curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
				} while (null != curNode);
				curNode = endNode;
				int writeCount;
				for (writeCount = length; writeCount > maxPath; writeCount--)
				{
					dtAssert(curNode);
					curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
				}
				for (int i = writeCount - 1; i >= 0; i--)
				{
					dtAssert(curNode);
					path[i] = curNode->id;
					curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
				}
				dtAssert(null == curNode);
				*pathCount = dtMin(length, maxPath);
				if (length > maxPath)
					return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
				return DT_SUCCESS;
			}
			public dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, float* startPos, float* endPos, dtQueryFilter* filter, uint options)
			{
				dtAssert(m_nav);
				dtAssert(m_nodePool);
				dtAssert(m_openList);
				memset(Unsafe.AsPointer(ref m_query), 0, sizeof(dtQueryData));
				m_query.status = DT_FAILURE;
				m_query.startRef = startRef;
				m_query.endRef = endRef;
				if (null != startPos)
					fixed (float* startPos1 = m_query.startPos)
						dtVcopy(startPos1, startPos);
				if (null != endPos)
					fixed (float* endPos1 = m_query.endPos)
						dtVcopy(endPos1, endPos);
				m_query.filter = filter;
				m_query.options = options;
				m_query.raycastLimitSqr = FLT_MAX;
				if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) ||
				null == startPos || !dtVisfinite(startPos) ||
				null == endPos || !dtVisfinite(endPos) || null == filter)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				if (0 != (options & (uint)dtFindPathOptions.DT_FINDPATH_ANY_ANGLE))
				{
					dtMeshTile* tile = m_nav->getTileByRef(startRef);
					float agentRadius = tile->header->walkableRadius;
					m_query.raycastLimitSqr = dtSqr(agentRadius * DT_RAY_CAST_LIMIT_PROPORTIONS);
				}
				if (startRef == endRef)
				{
					m_query.status = DT_SUCCESS;
					return DT_SUCCESS;
				}
				m_nodePool->clear();
				m_openList->clear();
				dtNode* startNode = m_nodePool->getNode(startRef);
				dtVcopy(startNode->pos, startPos);
				startNode->pidx = 0;
				startNode->cost = 0;
				startNode->total = dtVdist(startPos, endPos) * H_SCALE;
				startNode->id = startRef;
				startNode->flags = (uint)dtNodeFlags.DT_NODE_OPEN;
				m_openList->push(startNode);
				m_query.status = DT_IN_PROGRESS;
				m_query.lastBestNode = startNode;
				m_query.lastBestNodeCost = startNode->total;
				return m_query.status;
			}
			public dtStatus findStraightPath(float* startPos, float* endPos, dtPolyRef* path, int pathSize, float* straightPath, byte* straightPathFlags, dtPolyRef* straightPathRefs, ref int straightPathCount, int maxStraightPath, int options = 0)
			{
				dtAssert(m_nav);
				//if (0 == straightPathCount)
				//    return DT_FAILURE | DT_INVALID_PARAM;
				straightPathCount = 0;
				if (null == startPos || !dtVisfinite(startPos) ||
				null == endPos || !dtVisfinite(endPos) ||
				null == path || pathSize <= 0 || 0 == path[0] ||
				maxStraightPath <= 0)
				{
					return DT_FAILURE | DT_INVALID_PARAM;
				}
				dtStatus stat = 0;
				float* closestStartPos = stackalloc float[3];
				if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
					return DT_FAILURE | DT_INVALID_PARAM;
				float* closestEndPos = stackalloc float[3];
				if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize - 1], endPos, closestEndPos)))
					return DT_FAILURE | DT_INVALID_PARAM;
				stat = appendVertex(closestStartPos, (byte)dtStraightPathFlags.DT_STRAIGHTPATH_START, path[0],
				straightPath, straightPathFlags, straightPathRefs,
				ref straightPathCount, maxStraightPath);
				if (stat != DT_IN_PROGRESS)
					return stat;
				if (pathSize > 1)
				{
					float* portalApex = stackalloc float[3], portalLeft = stackalloc float[3], portalRight = stackalloc float[3];
					dtVcopy(portalApex, closestStartPos);
					dtVcopy(portalLeft, portalApex);
					dtVcopy(portalRight, portalApex);
					int apexIndex = 0;
					int leftIndex = 0;
					int rightIndex = 0;
					byte leftPolyType = 0;
					byte rightPolyType = 0;
					dtPolyRef leftPolyRef = path[0];
					dtPolyRef rightPolyRef = path[0];
					float* left = stackalloc float[3], right = stackalloc float[3];
					for (int i = 0; i < pathSize; ++i)
					{
						memset(left, 0, sizeof(float) * 3);
						memset(right, 0, sizeof(float) * 3);
						//float* left = stackalloc float[3], right = stackalloc float[3];
						byte toType = 0;
						if (i + 1 < pathSize)
						{
							byte fromType = 0;
							if (dtStatusFailed(getPortalPoints(path[i], path[i + 1], left, right, ref fromType, ref toType)))
							{
								if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
								{
									return DT_FAILURE | DT_INVALID_PARAM;
								}
								if (0 != (options & ((int)dtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS | (int)dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)))
								{
									appendPortals(apexIndex, i, closestEndPos, path,
									 straightPath, straightPathFlags, straightPathRefs,
									 ref straightPathCount, maxStraightPath, options);
								}
								appendVertex(closestEndPos, 0, path[i],
								straightPath, straightPathFlags, straightPathRefs,
								ref straightPathCount, maxStraightPath);
								return DT_SUCCESS | DT_PARTIAL_RESULT | ((straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
							}
							if (i == 0)
							{
								float t = 0f;
								if (dtDistancePtSegSqr2D(portalApex, left, right, ref t) < dtSqr(0.001f))
									continue;
							}
						}
						else
						{
							dtVcopy(left, closestEndPos);
							dtVcopy(right, closestEndPos);
							toType = (int)dtPolyTypes.DT_POLYTYPE_GROUND;
						}
						if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f)
						{
							if (dtVequal(portalApex, portalRight) || dtTriArea2D(portalApex, portalLeft, right) > 0.0f)
							{
								dtVcopy(portalRight, right);
								rightPolyRef = (i + 1 < pathSize) ? path[i + 1] : 0;
								rightPolyType = toType;
								rightIndex = i;
							}
							else
							{
								if (0 != (options & ((int)dtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS | (int)dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)))
								{
									stat = appendPortals(apexIndex, leftIndex, portalLeft, path,
									 straightPath, straightPathFlags, straightPathRefs,
									 ref straightPathCount, maxStraightPath, options);
									if (stat != DT_IN_PROGRESS)
										return stat;
								}
								dtVcopy(portalApex, portalLeft);
								apexIndex = leftIndex;
								byte flags = 0;
								if (0 == leftPolyRef)
									flags = (byte)dtStraightPathFlags.DT_STRAIGHTPATH_END;
								else if (leftPolyType == (int)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
									flags = (byte)dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION;
								dtPolyRef Ref = leftPolyRef;
								stat = appendVertex(portalApex, flags, Ref,
								straightPath, straightPathFlags, straightPathRefs,
								ref straightPathCount, maxStraightPath);
								if (stat != DT_IN_PROGRESS)
									return stat;
								dtVcopy(portalLeft, portalApex);
								dtVcopy(portalRight, portalApex);
								leftIndex = apexIndex;
								rightIndex = apexIndex;
								i = apexIndex;
								continue;
							}
						}
						if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f)
						{
							if (dtVequal(portalApex, portalLeft) || dtTriArea2D(portalApex, portalRight, left) < 0.0f)
							{
								dtVcopy(portalLeft, left);
								leftPolyRef = (i + 1 < pathSize) ? path[i + 1] : 0;
								leftPolyType = toType;
								leftIndex = i;
							}
							else
							{
								if (0 != (options & ((int)dtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS | (int)dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)))
								{
									stat = appendPortals(apexIndex, rightIndex, portalRight, path,
									 straightPath, straightPathFlags, straightPathRefs,
									 ref straightPathCount, maxStraightPath, options);
									if (stat != DT_IN_PROGRESS)
										return stat;
								}
								dtVcopy(portalApex, portalRight);
								apexIndex = rightIndex;
								byte flags = 0;
								if (0 == rightPolyRef)
									flags = (byte)dtStraightPathFlags.DT_STRAIGHTPATH_END;
								else if (rightPolyType == (int)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
									flags = (byte)dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION;
								dtPolyRef Ref = rightPolyRef;
								stat = appendVertex(portalApex, flags, Ref,
								straightPath, straightPathFlags, straightPathRefs,
								ref straightPathCount, maxStraightPath);
								if (stat != DT_IN_PROGRESS)
									return stat;
								dtVcopy(portalLeft, portalApex);
								dtVcopy(portalRight, portalApex);
								leftIndex = apexIndex;
								rightIndex = apexIndex;
								i = apexIndex;
								continue;
							}
						}
					}
					if (0 != (options & ((int)dtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS | (int)dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)))
					{
						stat = appendPortals(apexIndex, pathSize - 1, closestEndPos, path,
						 straightPath, straightPathFlags, straightPathRefs,
						 ref straightPathCount, maxStraightPath, options);
						if (stat != DT_IN_PROGRESS)
							return stat;
					}
				}
				appendVertex(closestEndPos, (byte)dtStraightPathFlags.DT_STRAIGHTPATH_END, 0,
				straightPath, straightPathFlags, straightPathRefs,
				ref straightPathCount, maxStraightPath);
				return DT_SUCCESS | ((straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
			}
		}
	}
}
